/*
 * ESP32双BTS7960电机差速控制 + OLED显示
 * 功能：
 * - 摇杆Y轴控制前进/后退速度
 * - 摇杆X轴控制转向差速
 * - OLED实时显示系统状态
 */

//------------------- OLED显示屏配置 -------------------
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

//------------------- 电机驱动引脚定义 -------------------
// 左电机
#define L_R_EN 16
#define L_L_EN 17
#define L_RPWM 18
#define L_LPWM 19

// 右电机
#define R_R_EN 26
#define R_L_EN 27
#define R_RPWM 32
#define R_LPWM 33

//------------------- 输入设备引脚定义 -------------------
#define JOY_X 34    // 摇杆X轴
#define JOY_Y 35    // 摇杆Y轴
#define SWITCH_PIN 5 // 开关引脚

//------------------- 参数配置 -------------------
#define PWM_FREQ 10000    // PWM频率(Hz)
#define PWM_RES 8         // 分辨率(bit)
#define DEADZONE 15       // 摇杆死区(ADC值)
#define MIN_PWM 50        // 最小启动PWM
#define MAX_PWM 255       // 最大PWM

//------------------- 全局变量 -------------------
int joyX, joyY;          // 摇杆校准值
int centerX, centerY;    // 摇杆中心点
String motorState = "stop";
int currentSpeed = 0;
unsigned long lastDisplayUpdate = 0;

void setup() {
  // 初始化串口
  Serial.begin(115200);
  
  //------------------- 初始化OLED -------------------
  Wire.begin(15, 13);  // ESP32 I2C引脚（SDA=15, SCL=13）
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("OLED初始化失败"));
    while(1);
  }
  display.clearDisplay();
  display.setTextColor(WHITE);
  
  // 显示开机动画
  displayStartupScreen();
  
  //------------------- 初始化电机控制 -------------------
  pinMode(L_R_EN, OUTPUT);
  pinMode(L_L_EN, OUTPUT);
  pinMode(R_R_EN, OUTPUT);
  pinMode(R_L_EN, OUTPUT);
  
  // 使能所有半桥
  digitalWrite(L_R_EN, HIGH);
  digitalWrite(L_L_EN, HIGH);
  digitalWrite(R_R_EN, HIGH);
  digitalWrite(R_L_EN, HIGH);

  // 配置PWM通道
  ledcSetup(0, PWM_FREQ, PWM_RES);  // 左电机正转
  ledcSetup(1, PWM_FREQ, PWM_RES);  // 左电机反转
  ledcSetup(2, PWM_FREQ, PWM_RES);  // 右电机正转
  ledcSetup(3, PWM_FREQ, PWM_RES);  // 右电机反转
  
  ledcAttachPin(L_RPWM, 0);
  ledcAttachPin(L_LPWM, 1);
  ledcAttachPin(R_RPWM, 2);
  ledcAttachPin(R_LPWM, 3);

  // 初始化开关
  pinMode(SWITCH_PIN, INPUT_PULLUP);

  // 摇杆校准
  calibrateJoystick();
  Serial.println("系统初始化完成！");
}

void loop() {
  // 1. 读取摇杆值
  readJoystick();
  
  // 2. 计算电机速度
  int baseSpeed = map(joyY, -1024, 1024, -MAX_PWM, MAX_PWM);
  int turnDiff = map(abs(joyX), 0, 1024, 0, MAX_PWM) * (joyX > 0 ? 1 : -1);
  
  // 3. 差速控制
  int leftSpeed = constrain(baseSpeed + turnDiff, -MAX_PWM, MAX_PWM);
  int rightSpeed = constrain(baseSpeed - turnDiff, -MAX_PWM, MAX_PWM);
  
  // 4. 更新电机状态显示
  updateMotorState(leftSpeed, rightSpeed);
  
  // 5. 驱动电机（检查开关状态）
  if(digitalRead(SWITCH_PIN) == LOW) {
    setMotor(0, leftSpeed);
    setMotor(1, rightSpeed);
  } else {
    setMotor(0, 0);
    setMotor(1, 0);
    motorState = "stop(switch)";
  }
  
  // 6. 更新OLED显示（每100ms更新一次）
  if(millis() - lastDisplayUpdate > 100) {
    updateDisplay();
    lastDisplayUpdate = millis();
  }
  
  delay(10);
}

//------------------- 功能函数 -------------------

// 显示开机动画
void displayStartupScreen() {
  display.clearDisplay();
  display.setTextSize(2);
  display.setCursor(0, 0);
  display.println(" EM CAR");
  display.println(" 9 GROUP");
  display.display();
  delay(2000);
}

// 更新OLED显示
void updateDisplay() {
  display.clearDisplay();
  display.setTextSize(1);
  
  // 摇杆状态
  display.setCursor(0, 0);
  display.print("X:"); display.print(joyX);
  display.print(" Y:"); display.println(joyY);
  
  // 开关状态
  display.print("stop:");
  display.println(digitalRead(SWITCH_PIN) == LOW ? "ON " : "OFF");
  
  // 电机状态
  display.print("state:"); display.println(motorState);
  
  // 速度显示
  display.print("speed:"); 
  display.print(currentSpeed); 
  display.println("%");
  
  // 电机PWM值
  display.print("L:"); display.print(map(abs(joyY + joyX), 0, 2048, 0, 100));
  display.print("% R:"); display.print(map(abs(joyY - joyX), 0, 2048, 0, 100));
  display.println("%");
  
  display.display();
}

// 更新电机状态
void updateMotorState(int left, int right) {
  currentSpeed = map(constrain(abs(joyY), 0, 1024), 0, 1024, 0, 100);
  
  if(abs(left) < 10 && abs(right) < 10) {
    motorState = "stop";
  } else if(left > 0 && right > 0) {
    motorState = "forward";
  } else if(left < 0 && right < 0) {
    motorState = "backward";
  } else if(left > right) {
    motorState = "turn left";
  } else {
    motorState = "turn right";
  }
}

// 摇杆校准函数
void calibrateJoystick() {
  centerX = 0;
  centerY = 0;
  for(int i=0; i<100; i++) {
    centerX += analogRead(JOY_X);
    centerY += analogRead(JOY_Y);
    delay(10);
  }
  centerX /= 100;
  centerY /= 100;
}

// 读取摇杆值(带死区处理)
void readJoystick() {
  int rawX = analogRead(JOY_X) - centerX;
  int rawY = analogRead(JOY_Y) - centerY;
  
  // 死区处理
  joyX = (abs(rawX) > DEADZONE) ? rawX : 0;
  joyY = (abs(rawY) > DEADZONE) ? rawY : 0;
}

// 电机控制函数
void setMotor(int motor, int speed) {
  speed = constrain(speed, -MAX_PWM, MAX_PWM);
  
  if(motor == 0) { // 左电机
    if(speed > 0) {
      ledcWrite(0, abs(speed));
      ledcWrite(1, 0);
    } else {
      ledcWrite(0, 0);
      ledcWrite(1, abs(speed));
    }
  } else { // 右电机
    if(speed > 0) {
      ledcWrite(2, abs(speed));
      ledcWrite(3, 0);
    } else {
      ledcWrite(2, 0);
      ledcWrite(3, abs(speed));
    }
  }
}